/*
 * Vision.h
 *
 *  Created on: Nov 13, 2013
 *      Author: parzival
 */

#ifndef VISION_H_
#define VISION_H_

#include <istream>
#include <stdio.h>
#include <math.h>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <cv.h>
#include <highgui.h>
//#include "MotorController.h"

using namespace cv;

//Motor Initialization
#define NUM_ACTUATOR 3
#define PI	3.141592f

#define STAND		0
#define WALK		1
#define TURNLEFT	2
#define TURNRIGHT	3
#define STEPLEFT	4
#define STEPRIGHT	5
#define KICK		6


#define TORQUE_ENABLE			24
#define MOVING_SPEED			32
#define GOAL_ACCELERATION		73

#define DEFAULT_MOVING_SPEED	50
#define DEFAULT_ACCELERATION	15
#define LOW_ACCELERATION		15
#define GOAL_POSITION			30
#define PRESENT_POSITION		36



 class Vision {
public:
	Vision();
	virtual ~Vision();
	int step(bool);
private:
	IplImage* GetThresholdedImage(IplImage*);
	int findPos(IplImage*, float, float);
	void moveHeadMotors(int gridPos);
	int getBodyMovement();
	int detectContours(IplImage*);
	void initMotorUSB();
	int setMotor(char);
	int setSpeed();
	int setCamera();
	void DrawLine(Mat, Point, Point);
	void DrawGridFull(IplImage*);
};

#endif /* VISION_H_ */
